#include <state.hpp>

namespace ohe_framework
{
//////////////////////////////////////////////////////////////////////////////////////////
    DynamicState::DynamicState()
    {
        x = 0;
        u = 0;
        a = 0;
    }
    void DynamicState::reset()
    {
        x = 0;
        u = 0;
        a = 0;
    }
/////////////////////////////////////////////////////////////////////////////////////////
    void Detection::resetHistory()
    {
        for(char i=0;i<HISTORY;i++)
        {
            detection_states[i]=START;
        }
        current_state = 0;
    }
    void Detection::setcurrentState(unsigned char state)
    {
        current_state = (current_state+1)%HISTORY;
        detection_states[current_state] = state;
    }
    unsigned char Detection::getCurrentState()
    {
        return detection_states[current_state];
    }
////////////////////////////////////////////////////////////////////////////////////////
    void getRadonTransform(ImageMatrixUchar &img, std::ofstream &file)
    {
        unsigned char *in_data= img.getDataPointer();
        unsigned int rows = img.getRows();
        unsigned int cols = img.getCols();
        unsigned int in_widthstep = img.getWidthstep();
        radon rad(rows,cols);
        for(unsigned int i =0;i<cols;i++)
        {
            int buff = 0;
            for(unsigned int j=0;j<rows;j++)
            {
                buff+=in_data[j*in_widthstep+i];
            }
            rad.row[i]=buff;
            file<<buff<<"\t";
        }
        file<<std::endl;
        /*for(unsigned int i =0;i<rows;i++)
        {
            int buff = 0;
            for(unsigned int j=0;j<cols;j++)
            {
                buff+=in_data[i*in_widthstep+j];
            }
            rad.col[i]=buff;
        }*/
    }
    void prepareRadonTransform(ImageMatrixUchar &img, Blob &upper, Blob &lower, std::ofstream &file)
    {
        Point center1;
        Point center2;
        ImageMatrixUchar gray_image;
        convertToGrayscale(img,gray_image);
        cv::Mat gray(cv::Size(img.getCols(),img.getRows()),CV_8UC1);
        gray_image.matrixToMat(gray);
        cv::blur(gray,gray,cv::Size(5,5));
        gray_image.matToMatrix(gray);
        upper.getBoundingRectangle(center1,center2);
        center1.x-=10;
        center1.y-=10;
        center2.x-=center1.x-10;
        center2.y-=center1.y-10;
        gray_image.setROI(center1,center2);
        getRadonTransform(gray_image,file);
        gray_image.removeROI();
        /*lower.getBoundingRectangle(center1,center2);
        center1.x-=10;
        center1.y-=10;
        center2.x-=center1.x-10;
        center2.y-=center1.y-10;
        gray_image.setROI(center1,center2);
        getRadonTransform(gray_image);
        gray_image.removeROI();*/
    }
    void separateContours(std::vector<Blob> &contours,
                          std::vector<std::vector<unsigned int> > &contour_index,Detection &state)
    {
        std::vector<unsigned int> wire;
        std::vector<unsigned int> mast;
        switch(state.getCurrentState())
        {
            case START:
                for(unsigned int i=0;i<contours.size();i++)
                {
                    Point start,end;
                    contours[i].getBoundingRectangle(start,end);
                    double ratio = (double)(end.x-start.x)/(end.y-start.y);
                    int area = (end.x-start.x)*(end.y-start.y);
                    if((ratio > 10)&&(area>100))
                    {
                        mast.push_back(i);
                    }
                    else
                    {
                        if(area<20)
                        {
                            //small_wire.push_back(i);
                        }
                        else
                        {
                            wire.push_back(i);
                        }
                    }
                }
                break;
            case WIRE:
                for(unsigned int i=0;i<contours.size();i++)
                {
                    Point start,end;
                    contours[i].getBoundingRectangle(start,end);
                    double ratio = (double)(end.x-start.x)/(end.y-start.y);
                    int area = (end.x-start.x)*(end.y-start.y);
                    if((ratio > 10)&&(area>100))
                    {
                        mast.push_back(i);
                    }
                    else
                    {
                        if(area<15)
                        {
                            //small_wire.push_back(i);
                        }
                        else
                        {
                            wire.push_back(i);
                        }
                    }
                }
                break;
            case WIRE_SECOND:
            //////// as above but with different conditions
                for(unsigned int i=0;i<contours.size();i++)
                {
                    Point start,end;
                    contours[i].getBoundingRectangle(start,end);
                    double ratio = (double)(end.x-start.x)/(end.y-start.y);
                    int area = (end.x-start.x)*(end.y-start.y);
                    if((ratio > 10)&&(area>100))
                    {
                        mast.push_back(i);
                    }
                    else
                    {
                        if(area<15)
                        {
                            //small_wire.push_back(i);
                        }
                        else
                        {
                            wire.push_back(i);
                        }
                    }
                }
                break;
            case MAST:
                for(unsigned int i=0;i<contours.size();i++)
                {
                    Point start,end;
                    contours[i].getBoundingRectangle(start,end);
                    double ratio = (double)(end.x-start.x)/(end.y-start.y);
                    int area = (end.x-start.x)*(end.y-start.y);
                    if((ratio > 10)&&(area>100))
                    {
                        mast.push_back(i);
                    }
                    else
                    {
                        if(area<20)
                        {
                            //small_wire.push_back(i);
                        }
                        else
                        {
                            wire.push_back(i);
                        }
                    }
                }
                break;
            case OBJECTS:
                for(unsigned int i=0;i<contours.size();i++)
                {
                    Point start,end;
                    contours[i].getBoundingRectangle(start,end);
                    double ratio = (double)(end.x-start.x)/(end.y-start.y);
                    int area = (end.x-start.x)*(end.y-start.y);
                    if((ratio > 10)&&(area>100))
                    {
                        mast.push_back(i);
                    }
                    else
                    {
                        if(area<20)
                        {
                            //small_wire.push_back(i);
                        }
                        else
                        {
                            wire.push_back(i);
                        }
                    }
                }
                break;
        }
        contour_index.push_back(wire);
        contour_index.push_back(mast);
    }
}
